#include "jtag.h"

#define ARM_NOP             0xE1A00000
#define ARM_MRS_R0_CPSR     0xE10F0000
#define ARM_MRS_R0_SPSR     0xE14F0000
#define ARM_LDR_R1_R0       0xE5901000
#define ARM_STR_R1_R0       0xE4801000

#define ARM_LDR_Rd_Rs(d,s) (0xE5900000 | ((d) << 12) | ((s) << 16))
#define ARM_MOV_Rx_0(x)    (0xE3A00000 | ((x) << 12))
#define ARM_STR_Rx_R14(x)  (0xE58E0000 | ((x) << 12))
#define ARM_LDR_Rx_R14(x)  (0xe59e0000 | ((x) << 12))

int jtag_halt_cpu(int reset)
{
    unsigned n, count;
    
    jtag_select_ice();
    
    if(reset) {
        jtag_srst(0);
    } else {
            /* see if we're already halted */
        if(jtag_ice_rd(ICE_DEBUG_STATUS) & 1) {
            return 0;
        }
    }
    
    jtag_ice_wr(ICE_WATCH0_ADDR_VALUE, 0x00000000);
    jtag_ice_wr(ICE_WATCH0_ADDR_MASK,  0xffffffff);
    jtag_ice_wr(ICE_WATCH0_DATA_VALUE, 0x00000000);
    jtag_ice_wr(ICE_WATCH0_DATA_MASK,  0xffffffff);
    jtag_ice_wr(ICE_WATCH0_CTRL_VALUE, 0x00000000);
    jtag_ice_wr(ICE_WATCH0_CTRL_MASK,  0xfffffff7);
    jtag_ice_wr(ICE_WATCH0_CTRL_VALUE, 0x00000100);
    
    if(reset) jtag_srst(1);
    
    for(count = 0; count < 1000; count++){
        n = jtag_ice_rd(ICE_DEBUG_STATUS);
        if((n & 9) == 9) {
            jtag_ice_wr(ICE_WATCH0_CTRL_VALUE, 0x00000000);
            jtag_reset();
            return 0;
        }
    }
    jtag_reset();
    return -1;
}

unsigned jtag_reg_rd(unsigned reg)
{
    jtag_dbg_io(ARM_STR_Rx_R14(reg), 0);
    jtag_dbg_io(ARM_NOP, 0);
    jtag_dbg_io(ARM_NOP, 0);
    return jtag_dbg_io(ARM_NOP, 0);
}

unsigned jtag_cpsr_rd()
{
    jtag_dbg_io(ARM_MRS_R0_CPSR, 0);
    return jtag_reg_rd(0);
}

void jtag_reg_wr(unsigned reg, unsigned val)
{
    jtag_dbg_io(ARM_MOV_Rx_0(14), 0);
    jtag_dbg_io(ARM_LDR_Rx_R14(reg), 0);
    jtag_dbg_io(ARM_NOP, 0);
    jtag_dbg_io(ARM_NOP, 0);
    jtag_dbg_io(val, 0);
    jtag_dbg_io(ARM_NOP, 0);
    if(reg == 15){
        jtag_dbg_io(ARM_NOP, 0);
        jtag_dbg_io(ARM_NOP, 0);
    }
}

int jtag_exec_wait(void)
{
    unsigned n;
    
    jtag_select_restart();
    
    jtag_select_ice();
    
    for(n = 0; n < 100; n++){
        if((jtag_ice_rd(ICE_DEBUG_STATUS) & 9) == 9) {
            jtag_select_debug();
            return 0;
        }
    }
    
    jtag_select_debug();
    return -1;
}

unsigned jtag_mem_rd(unsigned addr)
{
        /* stuff the address in R0 */
    jtag_reg_wr(0, addr);
    jtag_dbg_io(ARM_NOP, 0); // bug, can omit this I bet
    jtag_dbg_io(ARM_NOP, 1);
    jtag_dbg_io(ARM_LDR_R1_R0, 0); 
    jtag_dbg_io(ARM_NOP, 0);

    if(jtag_exec_wait()){
        return 0xffffffff;
    } else {
        return jtag_reg_rd(1);
    }
}

void jtag_mem_wr(unsigned addr, unsigned val)
{
    jtag_reg_wr(0, addr);
    jtag_reg_wr(1, val);
    jtag_dbg_io(ARM_NOP, 0); // bug, can omit this I bet
    jtag_dbg_io(ARM_NOP, 1);
    jtag_dbg_io(ARM_STR_R1_R0, 0); 
    jtag_dbg_io(ARM_NOP, 0);

    jtag_exec_wait();
}
